﻿using System;
using System.Collections.Generic;
using System.Windows;
using System.Windows.Controls;
using System.Linq;
using System.Windows.Shapes;
using System.Windows.Media;
using Microsoft.Kinect;
using DetectingKinectSencor;
using DetectingKinectSencor._40_Tools;
using DetectingKinectSencor.Tools;



namespace Kinect.Toolbox
{
    /// <summary>
    /// The following class was provided by the book 
    /// Microsoft.Press.Programming.with.the.Kinect.for.Windows.Software.Development.Kit.Sep.2012
    /// </summary>
    public class SkeletonDisplayManager
    {
        readonly Canvas rootCanvas;
        readonly KinectSensor sensor;
        private Vector3 leftShoulderPosition;
        private Vector3 rightShoulderPosition;
        private float differenceRightLeftShoulder;
        private float differenceLeftRightShoulder;
    
        public static int farthesPointLeftX;
        public static int farthesPointRightX;

        public static int farthesPointHipY;
        private bool positionSet;


        public static bool speakerOnRightSide { get; set; }



        ////////////////////////////////////////////////
        //               Constructor                  //
        ////////////////////////////////////////////////

        /// <summary>
        /// Constructor of SkeletonDisplayManager
        /// </summary>
        /// <param name="kinectSensor"></param>
        /// <param name="root"></param>
        public SkeletonDisplayManager(KinectSensor kinectSensor, Canvas root)
        {
            rootCanvas = root;
            sensor = kinectSensor;
        }

        public SkeletonDisplayManager(Canvas root)
        {
            rootCanvas = root;
        }

        ////////////////////////////////////////////////
        //          Method GetCoordiantes             //
        ////////////////////////////////////////////////

        /// <summary>
        /// 
        /// </summary>
        /// <param name="jointType"></param>
        /// <param name="joints"></param>
        /// <param name="x"></param>
        /// <param name="y"></param>
        void GetCoordinatesWithOffset(JointType jointType, IEnumerable<Joint> joints, out float x, out float y, float offsetX, float offsetY)
        {
            Joint joint = joints.First(j => j.JointType == jointType);

            Vector2 vector2 = SkeletonTools.Convert(sensor, joint.Position);


            x = (float)(vector2.X * rootCanvas.ActualWidth + offsetX * rootCanvas.ActualWidth);
            y = (float)(vector2.Y * rootCanvas.ActualHeight + offsetY * rootCanvas.ActualHeight);

        }

        void GetCoordinates(JointType jointType, IEnumerable<Joint> joints, out float x, out float y)
        {
            var joint = joints.First(j => j.JointType == jointType);
            Vector2 vector2 = SkeletonTools.Convert(sensor, joint.Position);
            x = (float)(vector2.X * rootCanvas.ActualWidth);
            y = (float)(vector2.Y * rootCanvas.ActualHeight);
        }


        ////////////////////////////////////////////////
        //               Method Draw                  //
        ////////////////////////////////////////////////

        /// <summary>
        /// This method draws the joints and creates the required shapes
        /// </summary>
        /// <param name="skeletons"></param>
        public void draw(Skeleton skeleton)
        {
            rootCanvas.Children.Clear();
            // Left upper parts
            plot(JointType.HandLeft, skeleton.Joints);
            trace(JointType.HandLeft, JointType.WristLeft, skeleton.Joints);

            plot(JointType.WristLeft, skeleton.Joints);
            trace(JointType.WristLeft, JointType.ElbowLeft, skeleton.Joints);

            plot(JointType.ElbowLeft, skeleton.Joints);
            trace(JointType.ElbowLeft, JointType.ShoulderLeft, skeleton.Joints);

            plot(JointType.ShoulderLeft, skeleton.Joints);
            trace(JointType.ShoulderLeft, JointType.ShoulderCenter, skeleton.Joints);

            plot(JointType.ShoulderCenter, skeleton.Joints);
            trace(JointType.ShoulderCenter, JointType.Head, skeleton.Joints);

            // Center
            Plot(JointType.Head, JointType.ShoulderCenter, skeleton.Joints);
            trace(JointType.ShoulderCenter, JointType.ShoulderRight, skeleton.Joints);

            //Right upper parts
            plot(JointType.ShoulderRight, skeleton.Joints);
            trace(JointType.ShoulderRight, JointType.ElbowRight, skeleton.Joints);

            plot(JointType.ElbowRight, skeleton.Joints);
            trace(JointType.ElbowRight, JointType.WristRight, skeleton.Joints);

            plot(JointType.WristRight, skeleton.Joints);
            trace(JointType.WristRight, JointType.HandRight, skeleton.Joints);

            plot(JointType.HandRight, skeleton.Joints);
            trace(JointType.ShoulderCenter, JointType.Spine, skeleton.Joints);

            // Center
            plot(JointType.Spine, skeleton.Joints);
            trace(JointType.Spine, JointType.HipCenter, skeleton.Joints);

            plot(JointType.HipCenter, skeleton.Joints);
            trace(JointType.HipCenter, JointType.HipLeft, skeleton.Joints);

            // Left lower parts
            plot(JointType.HipLeft, skeleton.Joints);
            trace(JointType.HipLeft, JointType.KneeLeft, skeleton.Joints);

            plot(JointType.KneeLeft, skeleton.Joints);
            trace(JointType.KneeLeft, JointType.AnkleLeft, skeleton.Joints);

            plot(JointType.AnkleLeft, skeleton.Joints);
            trace(JointType.AnkleLeft, JointType.FootLeft, skeleton.Joints);
            plot(JointType.FootLeft, skeleton.Joints);

            trace(JointType.HipCenter, JointType.HipRight, skeleton.Joints);

            // Right lower parts
            plot(JointType.HipRight, skeleton.Joints);
            trace(JointType.HipRight, JointType.KneeRight, skeleton.Joints);

            plot(JointType.KneeRight, skeleton.Joints);
            trace(JointType.KneeRight, JointType.AnkleRight, skeleton.Joints);

            plot(JointType.AnkleRight, skeleton.Joints);
            trace(JointType.AnkleRight, JointType.FootRight, skeleton.Joints);

            plot(JointType.FootRight, skeleton.Joints);
           
        }



        ////////////////////////////////////////////////
        //          Method Draw when no legs!          //
        ////////////////////////////////////////////////

        /// <summary>
        /// Draws the joints, when the seated mode is on.
        /// </summary>
        /// <param name="skeletons"></param>
        /// <param name="seated"></param>
        public void drawNoLegs(Skeleton skeleton)
        {
            rootCanvas.Children.Clear();


            // Calculate where the offsets should be drawn, depending if the left or the right shoulder is
            // nearer to the sensor

            leftShoulderPosition = Vector3.ToVector3(skeleton.Joints.Where(j => j.JointType ==
                                                    JointType.ShoulderLeft).First().Position);

            rightShoulderPosition = Vector3.ToVector3(skeleton.Joints.Where(j => j.JointType ==
                                                    JointType.ShoulderRight).First().Position);

            // Left shoulder nearer
            differenceRightLeftShoulder = rightShoulderPosition.Z - leftShoulderPosition.Z;

            // right shoulder nearer
            differenceLeftRightShoulder = leftShoulderPosition.Z - rightShoulderPosition.Z;



            // left shoulder is nearer
            if (differenceRightLeftShoulder > XMLproperties.thresholdToCalculateOffestForNewShoulderPosition)
            {
                // First is offset X, second is offset Y
                plotWithOffset(JointType.ShoulderRight, XMLproperties.offsetOpenGesture + XMLproperties.additionalOffsetSmall, 0, skeleton.Joints);

                plotWithOffset(JointType.ShoulderLeft, (XMLproperties.offsetOpenGesture * -1) + XMLproperties.additionalOffsetBig, 0, skeleton.Joints);
                positionSet = true;
            }
            else
            {
                if (differenceLeftRightShoulder > XMLproperties.thresholdToCalculateOffestForNewShoulderPosition)
                {
                    plotWithOffset(JointType.ShoulderRight, XMLproperties.offsetOpenGesture - XMLproperties.additionalOffsetBig, 0, skeleton.Joints);

                    plotWithOffset(JointType.ShoulderLeft, (XMLproperties.offsetOpenGesture * -1) - XMLproperties.additionalOffsetSmall, 0, skeleton.Joints);
                    positionSet = true;
                }
            }


            // If there is no rotation in the Y-axes then add the normal offset
            if (!positionSet)
            {
                plotWithOffset(JointType.ShoulderRight, XMLproperties.offsetOpenGesture, 0, skeleton.Joints);

                plotWithOffset(JointType.ShoulderLeft, (XMLproperties.offsetOpenGesture * -1), 0, skeleton.Joints);
            }


            // Set actual hip position
            plotWithOffset(JointType.HipLeft, 0, (XMLproperties.offsetHandsBelowWaistline * 1), skeleton.Joints);


            // Draw the rectangle which where the slides should be

            if (speakerOnRightSide)
            {
                // Line from hip to left border
                trace(farthesPointLeftX,            // source X
                    farthesPointHipY,               // source Y
                    0,                              // Destination X
                    farthesPointHipY);              // Destination Y

                // Line from hip to top border
                trace(farthesPointLeftX,            // source X
                    farthesPointHipY,                // source Y
                    farthesPointLeftX,              // Destination X
                    0);                             // Destination Y
            }
            else
            {
                // Line from hip to right border
                trace(farthesPointRightX,           // source X
                    farthesPointHipY,               // source Y
                    (float)rootCanvas.ActualWidth,  // Destination X
                    farthesPointHipY);              // Destination Y

                // Line from hip to top border
                trace(farthesPointRightX,           // source X
                    farthesPointHipY,               // source Y
                    farthesPointRightX,             // Destination X
                    0);                             // Destination Y
            }



            positionSet = false;

            // Left upper parts
            plot(JointType.HandLeft, skeleton.Joints);
            trace(JointType.HandLeft, JointType.WristLeft, skeleton.Joints);

            plot(JointType.WristLeft, skeleton.Joints);
            trace(JointType.WristLeft, JointType.ElbowLeft, skeleton.Joints);

            plot(JointType.ElbowLeft, skeleton.Joints);
            trace(JointType.ElbowLeft, JointType.ShoulderLeft, skeleton.Joints);

            plot(JointType.ShoulderLeft, skeleton.Joints);
            trace(JointType.ShoulderLeft, JointType.ShoulderCenter, skeleton.Joints);

            plot(JointType.ShoulderCenter, skeleton.Joints);
            trace(JointType.ShoulderCenter, JointType.Head, skeleton.Joints);

            // Center
            Plot(JointType.Head, JointType.ShoulderCenter, skeleton.Joints);
            trace(JointType.ShoulderCenter, JointType.ShoulderRight, skeleton.Joints);

            //Right upper parts
            plot(JointType.ShoulderRight, skeleton.Joints);
            trace(JointType.ShoulderRight, JointType.ElbowRight, skeleton.Joints);

            plot(JointType.ElbowRight, skeleton.Joints);
            trace(JointType.ElbowRight, JointType.WristRight, skeleton.Joints);

            plot(JointType.WristRight, skeleton.Joints);
            trace(JointType.WristRight, JointType.HandRight, skeleton.Joints);

            plot(JointType.HandRight, skeleton.Joints);
            trace(JointType.ShoulderCenter, JointType.Spine, skeleton.Joints);

            // Center
            plot(JointType.Spine, skeleton.Joints);
            trace(JointType.Spine, JointType.HipCenter, skeleton.Joints);

            plot(JointType.HipCenter, skeleton.Joints);
            trace(JointType.HipCenter, JointType.HipLeft, skeleton.Joints);

            // Left lower parts
            plot(JointType.HipLeft, skeleton.Joints);
/*              Trace(JointType.HipLeft, JointType.KneeLeft, skeleton.Joints);

            Plot(JointType.KneeLeft, skeleton.Joints);
            Trace(JointType.KneeLeft, JointType.AnkleLeft, skeleton.Joints);

            Plot(JointType.AnkleLeft, skeleton.Joints);
            Trace(JointType.AnkleLeft, JointType.FootLeft, skeleton.Joints);
            Plot(JointType.FootLeft, skeleton.Joints);
*/
            trace(JointType.HipCenter, JointType.HipRight, skeleton.Joints);

            // Right lower parts
            plot(JointType.HipRight, skeleton.Joints);
/*              Trace(JointType.HipRight, JointType.KneeRight, skeleton.Joints);

            Plot(JointType.KneeRight, skeleton.Joints);
            Trace(JointType.KneeRight, JointType.AnkleRight, skeleton.Joints);

            Plot(JointType.AnkleRight, skeleton.Joints);
            Trace(JointType.AnkleRight, JointType.FootRight, skeleton.Joints);

            Plot(JointType.FootRight, skeleton.Joints);
 * 
 * 
*/  
        }








        ////////////////////////////////////////////////
        //               Method Plot                  //
        ////////////////////////////////////////////////

        /// <summary>
        /// The plot method draws a point where the joint belongs
        /// </summary>
        /// <param name="centerID"></param>
        /// <param name="joints"></param>
        void plot(JointType centerID, IEnumerable<Joint> joints){
            float centerX;
            float centerY;
            GetCoordinates(centerID, joints, out centerX, out centerY);
            const double diameter = 8;
            Ellipse ellipse = new Ellipse
            {
                Width = diameter,
                Height = diameter,
                HorizontalAlignment = HorizontalAlignment.Left,
                VerticalAlignment = VerticalAlignment.Top,
                StrokeThickness = 4.0,
                Stroke = new SolidColorBrush(Colors.Green),
                StrokeLineJoin = PenLineJoin.Round
            };
            Canvas.SetLeft(ellipse, centerX - ellipse.Width / 2);
            Canvas.SetTop(ellipse, centerY - ellipse.Height / 2);
            rootCanvas.Children.Add(ellipse);
        }






        /// <summary>
        /// Special plot when an offset needs to be plotted, shoulder and hip
        /// </summary>
        /// <param name="centerID"></param>
        /// <param name="offsetX"></param>
        /// <param name="offsetY"></param>
        /// <param name="joints"></param>
        void plotWithOffset(JointType centerID, float offsetX, float offsetY, IEnumerable<Joint> joints)
        {
            float centerX;
            float centerY;
            GetCoordinatesWithOffset(centerID, joints, out centerX, out centerY, offsetX, offsetY);
            const double diameter = 13;
            Ellipse ellipse = new Ellipse
            {
                Width = diameter,
                Height = diameter,
                HorizontalAlignment = HorizontalAlignment.Left,
                VerticalAlignment = VerticalAlignment.Top,
                StrokeThickness = 4.0,
                Stroke = new SolidColorBrush(Colors.Red),
                StrokeLineJoin = PenLineJoin.Round
            };
            Canvas.SetLeft(ellipse, centerX - ellipse.Width / 2);
            Canvas.SetTop(ellipse, centerY - ellipse.Height / 2);

            rootCanvas.Children.Add(ellipse);


        }



        ////////////////////////////////////////////////
        //               Method Plot                  //
        ////////////////////////////////////////////////

        /// <summary>
        /// This methos is used for the head, because there is a real cicle plotted, 
        /// which is bigger than the cicle of the joints
        /// </summary>
        /// <param name="centerID"></param>
        /// <param name="baseID"></param>
        /// <param name="joints"></param>
        void Plot(JointType centerID, JointType baseID, JointCollection joints)
        {
            float centerX;
            float centerY;
            GetCoordinates(centerID, joints, out centerX, out centerY);
            float baseX;
            float baseY;
            GetCoordinates(baseID, joints, out baseX, out baseY);
            double diameter = Math.Abs(baseY - centerY);
            Ellipse ellipse = new Ellipse
            {
                Width = diameter,
                Height = diameter,
                HorizontalAlignment = HorizontalAlignment.Left,
                VerticalAlignment = VerticalAlignment.Top,
                StrokeThickness = 4.0,
                Stroke = new SolidColorBrush(Colors.Green),
                StrokeLineJoin = PenLineJoin.Round
            };
            Canvas.SetLeft(ellipse, centerX - ellipse.Width / 2);
            Canvas.SetTop(ellipse, centerY - ellipse.Height / 2);
            rootCanvas.Children.Add(ellipse);
        }



        ////////////////////////////////////////////////
        //               Method Trace                 //
        ////////////////////////////////////////////////

        /// <summary>
        /// This method traces a line between two joints
        /// </summary>
        /// <param name="sourceID"></param>
        /// <param name="destinationID"></param>
        /// <param name="joints"></param>
        void trace(JointType sourceID, JointType destinationID, JointCollection joints){
            float sourceX;
            float sourceY;
            GetCoordinates(sourceID, joints, out sourceX, out sourceY);

            float destinationX;
            float destinationY;
            GetCoordinates(destinationID, joints, out destinationX, out destinationY);
            
            Line line = new Line
                            {
                                X1 = sourceX,
                                Y1 = sourceY,
                                X2 = destinationX,
                                Y2 = destinationY,
                                HorizontalAlignment = HorizontalAlignment.Left,
                                VerticalAlignment = VerticalAlignment.Top,
                                StrokeThickness = 4.0,                                
                                Stroke = new SolidColorBrush(Colors.Green),
                                StrokeLineJoin = PenLineJoin.Round
                            };
            rootCanvas.Children.Add(line);
        }



        /// <summary>
        /// This method traces a line to draw the rectangle
        /// </summary>
        /// <param name="sourceID"></param>
        /// <param name="destinationID"></param>
        /// <param name="joints"></param>
        void trace(float sourceX, float sourceY, float destinationX, float destinationY)
        {
            Line line = new Line
            {
                X1 = sourceX,
                Y1 = sourceY,
                X2 = destinationX,
                Y2 = destinationY,
                HorizontalAlignment = HorizontalAlignment.Left,
                VerticalAlignment = VerticalAlignment.Top,
                StrokeThickness = 4.0,
                Stroke = new SolidColorBrush(Colors.Red),
                StrokeLineJoin = PenLineJoin.Round
            };
            rootCanvas.Children.Add(line);
        }


        /// <summary>
        /// Returns the color image coordinate x and y of the transferred skeleton type
        /// </summary>
        /// <param name="jointType"></param>
        /// <param name="skeleton"></param>
        /// <returns></returns>
        internal Vector2 getColorPoint(JointType jointType, Skeleton skeleton)
        {
            Vector2 colorPoint = new Vector2();

            GetCoordinates(jointType, skeleton.Joints, out colorPoint.X, out colorPoint.Y);

            return colorPoint;
        }
    }
}
